#include "asr_softiic.h"


#define PCA9685_SUBADR1 0x2
#define PCA9685_SUBADR2 0x3
#define PCA9685_SUBADR3 0x4

#define PCA9685_MODE1 0x0
#define PCA9685_PRESCALE 0xFE

#define LED0_ON_L 0x6
#define LED0_ON_H 0x7
#define LED0_OFF_L 0x8
#define LED0_OFF_H 0x9

#define ALLLED_ON_L 0xFA
#define ALLLED_ON_H 0xFB
#define ALLLED_OFF_L 0xFC
#define ALLLED_OFF_H 0xFD



class Adafruit_PWMServoDriver {
 public:
  Adafruit_PWMServoDriver(uint8_t addr = 0x40);
  void begin(void);
  void reset(void);
  void setPWMFreq(float freq);
  void setPWM(uint8_t num, uint16_t on, uint16_t off);
  void setPin(uint8_t num, uint16_t val, bool invert=false);

 private:
  uint8_t _i2caddr;

  uint8_t read8(uint8_t addr);
  void write8(uint8_t addr, uint8_t d);
};





// class MySoftIIC
// {
//   public:
//   void begin(){softiic.begin(0,1);}
//   void stop(){softiic.stop();}
//   void write(uint8_t d){softiic.write(d);}
//   uint8_t read(bool b ){softiic.read(b);}
//   void requestFrom(uint8_t addr,uint8_t d){};
  
//   int start(uint8_t addr){return softiic.start(addr<<1|0);}
//   void beginTransmission(uint8_t addr){start(addr);}
//   void stop(){softiic.stop();}
// };


// MySoftIIC WIRE;


// Set to true to print some debug messages, or false to disable them.
#define ENABLE_DEBUG_OUTPUT false

Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(uint8_t addr) {
  _i2caddr = addr;
}

void Adafruit_PWMServoDriver::begin(void) {
 softiic.begin(0,1);
 reset();
}


void Adafruit_PWMServoDriver::reset(void) {
 write8(PCA9685_MODE1, 0x0);
}

void Adafruit_PWMServoDriver::setPWMFreq(float freq) {
  //Serial.print("Attempting to set freq ");
  //Serial.println(freq);
  freq *= 0.9;  // Correct for overshoot in the frequency setting (see issue #11).
  float prescaleval = 25000000;
  prescaleval /= 4096;
  prescaleval /= freq;
  prescaleval -= 1;
  if (ENABLE_DEBUG_OUTPUT) {
    Serial.print("Estimated pre-scale: "); Serial.println(prescaleval);
  }
  uint8_t prescale = floor(prescaleval + 0.5);
  if (ENABLE_DEBUG_OUTPUT) {
    Serial.print("Final pre-scale: "); Serial.println(prescale);
  }
  
  uint8_t oldmode = read8(PCA9685_MODE1);
  uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep
  write8(PCA9685_MODE1, newmode); // go to sleep
  write8(PCA9685_PRESCALE, prescale); // set the prescaler
  write8(PCA9685_MODE1, oldmode);
  delay(5);
  write8(PCA9685_MODE1, oldmode | 0xa1);  //  This sets the MODE1 register to turn on auto increment.
                                          // This is why the beginTransmission below was not working.
  //  Serial.print("Mode now 0x"); Serial.println(read8(PCA9685_MODE1), HEX);
}

void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) {
  //Serial.print("Setting PWM "); Serial.print(num); Serial.print(": "); Serial.print(on); Serial.print("->"); Serial.println(off);

  softiic.start(_i2caddr<<1|0);
  softiic.write(LED0_ON_L+4*num);
  softiic.write(on);
  softiic.write(on>>8);
  softiic.write(off);
  softiic.write(off>>8);
  softiic.stop();
}

// Sets pin without having to deal with on/off tick placement and properly handles
// a zero value as completely off.  Optional invert parameter supports inverting
// the pulse for sinking to ground.  Val should be a value from 0 to 4095 inclusive.
void Adafruit_PWMServoDriver::setPin(uint8_t num, uint16_t val, bool invert)
{
  // Clamp value between 0 and 4095 inclusive.
  if(val > 4095)
    val = 4095;
  if (invert) {
    if (val == 0) {
      // Special value for signal fully on.
      setPWM(num, 4096, 0);
    }
    else if (val == 4095) {
      // Special value for signal fully off.
      setPWM(num, 0, 4096);
    }
    else {
      setPWM(num, 0, 4095-val);
    }
  }
  else {
    if (val == 4095) {
      // Special value for signal fully on.
      setPWM(num, 4096, 0);
    }
    else if (val == 0) {
      // Special value for signal fully off.
      setPWM(num, 0, 4096);
    }
    else {
      setPWM(num, 0, val);
    }
  }
}

uint8_t Adafruit_PWMServoDriver::read8(uint8_t addr) {
  softiic.start(_i2caddr<<1|0);
  softiic.write(addr);
  softiic.stop();

  //softiic.requestFrom((uint8_t)_i2caddr, (uint8_t)1);
  //return softiic.read();
  return 0;
}

void Adafruit_PWMServoDriver::write8(uint8_t addr, uint8_t d) {
  softiic.start(_i2caddr<<1|0);
  softiic.write(addr);
  softiic.write(d);
  softiic.stop();
}



Adafruit_PWMServoDriver PCA9865;

//--------------------------------------------------------

///**************************************************************
bool bl = 0;
int i=0;
void LED(){
  while (1) {
    
    //ɨ豸 
    for(int i = 1;i<0x7f;i++)
    {
      // int val = softiic.start(i<<1 | 0);
      // softiic.stop();
      // Serial.print(i,HEX);
      // Serial.print("ox ");
      // Serial.println(val,HEX);
      pwm.begin();
      pwm.setPWMFreq(1600);
      pwm.setPin(4,i*46,1);
      delay(100);
      
    }
    
  
  }
  vTaskDelete(NULL);
}
//------------------------------------------------